/***************************************/
/********* Jose Delgado Perez **********/
/********* josedelgado@ula.ve **********/
/*********    LaSDAI - ULA    **********/
/*********     06-08-2011     **********/
/***************************************/

#include "controladoraPololu6.h"

#define delay 500000


void inicializacion(){
	asignarPosicion(2045, 2);
	usleep(delay);
	asignarPosicion(1308, 3);
	usleep(delay);
}

void extraerFila1(){
	asignarPosicion(1476, 1);
	usleep(delay);
	asignarPosicion(1510, 2);
	usleep(delay);
	asignarPosicion(1614, 3);
	usleep(delay);
	asignarPosicion(2380, 0);
	usleep(2000000);
	asignarPosicion(1350, 1);
	usleep(2*delay);
	asignarPosicion(1500, 1);
	usleep(delay);
	asignarPosicion(1320, 0);
	usleep(delay);
}

void extraerFila2(){
	asignarPosicion(1570, 1);
	usleep(delay);
	asignarPosicion(1790, 2);
	usleep(delay);
	asignarPosicion(1492, 3);
	usleep(delay);
	asignarPosicion(2528, 0);	
	usleep(2000000);
	asignarPosicion(1420, 1);
	usleep(2*delay);
	asignarPosicion(1600, 1);
	usleep(delay);
	asignarPosicion(1320, 0);
	usleep(delay);
}

void extraerFila3(){
	asignarPosicion(1610, 1);
	usleep(delay);
	asignarPosicion(2100, 2);
	usleep(delay);
	asignarPosicion(1250, 3);
	usleep(delay);
	asignarPosicion(2328, 0);	
	usleep(2000000);
	asignarPosicion(1380, 1);
	usleep(2*delay);
	asignarPosicion(1630, 1);
	usleep(delay);
	asignarPosicion(1320, 0);
	usleep(delay);
}

void descargarIzq(int fila){
	int i, aux;	
	int aux2[3] = {1510,1790,2100};
	asignarPosicion(1640, 1);
	usleep(delay);
	asignarPosicion(1245, 3);
	usleep(delay);
	aux = aux2[fila-1];	
	if(aux < 2000){
		i = 1;
	}else{
		i = -1;	
	}
	while((aux <= 2000 && i == 1) || (aux >= 2000 && i == -1)){
		aux = aux+10*i;
		asignarPosicion(aux, 2);
		usleep(10000);		
	}
	asignarPosicion(2300, 0);
	usleep(delay);
	asignarPosicion(2352, 3);
	usleep(1000000);
	asignarPosicion(1320, 0);
	usleep(delay);
}

void descargarDer(int fila){
	int i, aux;	
	int aux2[3] = {1510,1790,2100};
	asignarPosicion(1640, 1);
	usleep(delay);
	asignarPosicion(1245, 3);
	usleep(delay);
	aux = aux2[fila-1];	
	if(aux < 2000){
		i = 1;
	}else{
		i = -1;	
	}
	while((aux <= 2000 && i == 1) || (aux >= 2000 && i == -1)){
		aux = aux+10*i;
		asignarPosicion(aux, 2);
		usleep(10000);		
	}
	asignarPosicion(500, 0);
	usleep(delay);
	asignarPosicion(2352, 3);
	usleep(1000000);
	asignarPosicion(1320, 0);
	usleep(delay);
}

void posicionInicial(){
	/*asignarPosicion(1320, 0);
	usleep(delay);
	asignarPosicion(2352, 3);
	usleep(delay);
	asignarPosicion(2320, 2);	
	usleep(delay);	
	asignarPosicion(2016, 1);
	*/
	usleep(5000000);
	asignarPosicion(0, 0);
	asignarPosicion(0, 1);
	asignarPosicion(0, 2);
	asignarPosicion(0, 3);
}

void recogerColumna(){
	inicializacion();
	extraerFila1();
	descargarIzq(1);	
	extraerFila2();
	descargarDer(2);	
        extraerFila3();
	descargarIzq(3);	
	posicionInicial();
}



